#include "rs232.h"
#include "rs232.c"


class mouseDelta
{
	public:
	int dx;
	int dy;
};

void drukaf(unsigned char buffer[255]);
void goForward(unsigned char speed);
void goLeft(unsigned char speed, int time);
void goRight(unsigned char speed, int time);
void goBackward(unsigned char speed);
void stuurCommand(char command);
void getReply(int bytes);
void stop();
void testenMaar();
void Drive(int snelheid);
void Rotate(int hoek, bool clockwise);
mouseDelta getMouse(FILE *fmouse);

#define WINDOW 11
#define TRESHOLD 200
#define POORT 1		//Poort voor Beagleboard = 2
#define MAXV 0x40


void Drive(int snelheid)
{
	int res = OpenComport(POORT, 19200);
	float speed = MAXV*snelheid*0.01;
	if(res == 1)
	{
		printf("Error while opening comport\n");
		return;
	}

	unsigned char buf[3];

	buf[0] = 0xD6;
	buf[1] = speed;		//0xD9 = forward, 0xD6 is achteruit
	buf[2] = speed;
	res = SendBuf(POORT, buf, 3);
	if(res != 1)
	{
		if(snelheid == 0)
			printf("Stopped\n");
		else
			printf("Going forward, speed %d\n", (int)speed);
	}
	else
		printf("Error while trying to move.\n");
	CloseComport(POORT);
}

void Rotate(int hoek, bool clockwise)
{
	int res = OpenComport(POORT, 19200);
	
	if(res == 1)
	{
		printf("Error while opening comport\n");
		return;
	}

	unsigned char buf[3];

	if(clockwise)
		buf[0] = 0xD5;
	else
		buf[0] = 0xDA;
	buf[1] = MAXV;		//0xD9 = forward, 0xD6 is achteruit
	buf[2] = MAXV;
	res = SendBuf(POORT, buf, 3);
	if(res != 1)
		printf("Rotating...\n");
	else
		printf("Error while trying to rotate.\n");
	CloseComport(POORT);
	sleep(2);
	Drive(0);

}

void testenMaar()
{
	goForward(0x20);
	sleep(1);
	stuurCommand(0x8F);
	getReply(3);
	sleep(1);
	goBackward(0x30);
	sleep(2);
	stop();
}

void goForward(unsigned char speed)
{
	int res;
	unsigned char buf[3];

		
	buf[0] = 0xE9;		//0xD9 = forward, 0xD6 is achteruit
	buf[1] = speed;
	buf[2] = speed;
	res = SendBuf(POORT, buf, 3);
	if(res != 1)
		printf("Go forward, speed %c\n", buf[1]);
	else
		printf("FAILED to go forward\n");
}

void goLeft(unsigned char speed, int time)
{
	int res;
	unsigned char buf[2];

		
	buf[0] = 0xC1;	
	buf[1] = speed;
	res = SendBuf(POORT, buf, 2);
	if(res != 1)
		printf("Go left, speed %x\n", speed);
	else
		printf("FAILED to go left\n");
	//sleep(time);
	//stop();
}

void goRight(unsigned char speed, int time)
{
	int res;
	unsigned char buf[2];

		
	buf[0] = 0xCA;	
	buf[1] = speed;
	res = SendBuf(POORT, buf, 2);
	if(res != 1)
		printf("Go right, speed %x\n", speed);
	else
		printf("FAILED to go right\n");

	//sleep(time);
	//stop();
}

void goBackward(unsigned char speed)
{
	int res;
	unsigned char buf[3];

		
	buf[0] = 0xE6;		//0xD9 = forward, 0xD6 is achteruit
	buf[1] = speed;
	buf[2] = speed;
	res = SendBuf(POORT, buf, 3);
	if(res != 1)
		printf("Go backward, speed %x\n", speed);
	else
		printf("FAILED to go forward\n");
}


void stuurCommand(char command)
{
	int res;
	unsigned char buf;
	buf = 0x81;
	res = SendByte(POORT, buf);
	if(res != 1)
		printf("Commando gestuurd: %c\n", command);
	else
		printf("FAILED to send command\n");
}

void getReply(int bytes)
{
	unsigned char buffer[2];
	int i, n;
	for(i = 0; i < bytes; i++)
	{
		memset(buffer, '\0', 1);
		n = read(Cport[POORT], buffer, 1);
		if(n != 0)
		{
			printf("Reply: %x, n = %d\n", buffer[1], n);
		}
	}
}

void stop()
{
	int res;
	unsigned char buf[3];
	buf[0] = 0xDC;	
	buf[1] = 0;
	buf[2] = 0;
	res = SendBuf(POORT, buf, 3);
	if(res != 1)
		printf("Stopped\n");
	else
		printf("FAILED to stop\n");
}


mouseDelta getMouse(FILE *fmouse)
{
	
    char b[3];
    mouseDelta mickey;
        
	if(fmouse == 0)
	{
		printf("FAILED opening mousedevice\n");
		return mickey;
	}
		
	fread(b,sizeof(char),3,fmouse);
	//printf("pip: dx = %d, dy = %d\n", (int)b[1], atoi(b[2]));
	if(b[1] > 127)
	{
		mickey.dx = b[1] - 255;
	}
	else
	{
		mickey.dx = b[1];
	}
	if(b[2] > 127)
	{
		mickey.dy = b[2] - 255;
	}
	else
	{
		mickey.dy = b[2];
	}


	return mickey;
}



